using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Kinect;

namespace TableTennis
{

    /// <summary>
    /// This is the main type for your game
    /// </summary>
    public class Game1 : Microsoft.Xna.Framework.Game
    {
        GraphicsDeviceManager graphics;
        SpriteBatch spriteBatch;

        KinectSensor MyKinect;
        Skeleton[] allSkeletons = new Skeleton[6];

        SpriteFont Font;

        TopDownBody myBody;

        string ConnectedStatus = "";

        public Game1()
        {
            graphics = new GraphicsDeviceManager(this);
            Content.RootDirectory = "Content";
        }

        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // TODO: Add your initialization logic here
            KinectSensor.KinectSensors.StatusChanged += new EventHandler<StatusChangedEventArgs>(KinectSensors_StatusChanged);
            DiscoverKinectSensor();


            base.Initialize();
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            // Create a new SpriteBatch, which can be used to draw textures.
            spriteBatch = new SpriteBatch(GraphicsDevice);

            Font = Content.Load<SpriteFont>("Font");
        }

        private void DiscoverKinectSensor()
        {
            foreach (KinectSensor sensor in KinectSensor.KinectSensors)
            {
                if (sensor.Status == KinectStatus.Connected)
                {
                    // YAY! We found a connected Kinect! (excuse the loopy pronunciation)
                    MyKinect = sensor;
                }

                switch (sensor.Status)
                {
                    case KinectStatus.Connected:
                        {
                            ConnectedStatus = "Status: Connected";
                            break;
                        }
                    case KinectStatus.Disconnected:
                        {
                            ConnectedStatus = "Status: Disconnected";
                            break;
                        }
                    case KinectStatus.NotPowered:
                        {
                            ConnectedStatus = "Status: CONNECT THE FRIGGIN' POWER";
                            break;
                        }
                    default:
                        {
                            ConnectedStatus = "Status: ERROR. IDIOT.";
                            break;
                        }
                }
            }

            //if (this.MyKinect == null)
            //{
            //    ConnectedStatus = "No Kinect Sensors connected. Bah.";
            //    return;
            //}



            try
            {
                if (MyKinect.Status == KinectStatus.Connected)
                {
                    InitializeKinect();
                }
            }
            catch (NullReferenceException e)
            {
                string stt = e.Message;
            }
        }

        private bool InitializeKinect()
        {
            // Smoothing
            var parameters = new TransformSmoothParameters
            {
                Smoothing = 0.5f,
                Correction = 0.7f,
                Prediction = 0.5f,
                JitterRadius = 0.5f,
                MaxDeviationRadius = 0.4f
            };
            // Events
            MyKinect.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(MyKinect_AllFramesReady);
            MyKinect.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            MyKinect.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            MyKinect.SkeletonStream.Enable(parameters);

            try
            {
                MyKinect.Start();
                return true;
            }
            catch (MissingMemberException e)
            {
                ConnectedStatus = "UNABLE TO START THE SENSOR. LoL.";
                return false;
            }
        }

        void MyKinect_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            // Get first player.
            Skeleton first = GetFirstSkeleton(e);
            if (first == null)
                return;

            // Do stuff here.
            SetColorPoints(GetFirstSkeleton(e), e);
        }

        /// <summary>
        /// Scales 640x480 to desired mapping
        /// </summary>
        /// <param name="Cursor">Sprite to scale.</param>
        private void ScalePosition(Sprite Cursor)
        {
            Vector2 CursorPosition = Cursor.Position;
            // The formula:
            double x = CursorPosition.X * 1500 / 640 - 115;
            double y = CursorPosition.Y * 900 / 480 - 250;
            Cursor.Position = new Vector2((float)x - Cursor.width / 2, (float)y - Cursor.height / 2);

        }

        /// <summary>
        /// The actual "Kinect" logic.
        /// </summary>
        /// <param name="first">Player controlling program.</param>
        /// <param name="e">All frames ready.</param>
        private void SetColorPoints(Skeleton first, AllFramesReadyEventArgs e)
        {
            using (DepthImageFrame depth = e.OpenDepthImageFrame())
            {
                if (depth == null || MyKinect == null)
                {
                    return;
                }

                DepthImagePoint[] allDPoints = new DepthImagePoint[first.Joints.Count];
                ColorImagePoint[] allCPoints = new ColorImagePoint[first.Joints.Count];

                for (int i = 0; i < first.Joints.Count; i++)
                {
                    allDPoints[i] = depth.MapFromSkeletonPoint(first.Joints[(JointType)i].Position);
                    allCPoints[i] = depth.MapToColorImagePoint(allDPoints[i].X, allDPoints[i].Y, ColorImageFormat.RgbResolution640x480Fps30);
                }

                //DepthImagePoint headDepthPoint = depth.MapFromSkeletonPoint(first.Joints[JointType.Head].Position);
                //DepthImagePoint leftHandDepthPoint = depth.MapFromSkeletonPoint(first.Joints[JointType.HandLeft].Position);
                //DepthImagePoint rightHandDepthPoint = depth.MapFromSkeletonPoint(first.Joints[JointType.HandRight].Position);
                //// Depth to color
                //ColorImagePoint headColorPoint = depth.MapToColorImagePoint(headDepthPoint.X, headDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);
                //ColorImagePoint leftHandColorPoint = depth.MapToColorImagePoint(leftHandDepthPoint.X, leftHandDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);
                //ColorImagePoint rightHandColorPoint = depth.MapToColorImagePoint(rightHandDepthPoint.X, rightHandDepthPoint.Y, ColorImageFormat.RgbResolution640x480Fps30);

                //CameraPosition(CursorRight, rightHandColorPoint);
                //CameraPosition(CursorLeft, leftHandColorPoint);

                myBody = new TopDownBody(allCPoints, allDPoints, GraphicsDevice, Content);
            }
        }

        private void CameraPosition(Sprite MySprite, ColorImagePoint MyColorImagePoint)
        {
            // Centering
            MySprite.Position = new Vector2(MyColorImagePoint.X - MySprite.height / 2, MyColorImagePoint.Y - MySprite.height / 2);
        }

        private Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)
        {
            // Getting the first player among those playing.
            // To avoid confusion.
            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null)
                {
                    return null;
                }

                skeletonFrameData.CopySkeletonDataTo(allSkeletons);

                Skeleton first = (from s in allSkeletons where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault();

                return first;
            }
        }

        /// <summary>
        /// If a new Kinect is introduced.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void KinectSensors_StatusChanged(object sender, StatusChangedEventArgs e)
        {
            if (this.MyKinect == e.Sensor)
            {
                if (e.Status == KinectStatus.Disconnected || e.Status == KinectStatus.NotPowered)
                {
                    this.MyKinect = null;
                    this.DiscoverKinectSensor();
                }
            }
        }

        /// <summary>
        /// UnloadContent will be called once per game and is the place to unload
        /// all content.
        /// </summary>
        protected override void UnloadContent()
        {
            // TODO: Unload any non ContentManager content here
        }

        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            // Allows the game to exit
            if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
                this.Exit();

            // TODO: Add your update logic here

            base.Update(gameTime);
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            GraphicsDevice.Clear(Color.CornflowerBlue);

            // TODO: Add your drawing code here
            spriteBatch.Begin();

            if(myBody != null)
                myBody.DrawBody(spriteBatch);
            spriteBatch.DrawString(Font, ConnectedStatus, Vector2.Zero, Color.White);
            String str = (myBody != null) ? ("\nRightElbow: " + myBody.thisSkelDepth[(int)JointType.ElbowRight].Depth +
                "\n" + myBody.convertedDepth(JointType.ElbowRight)) : "";
            
            spriteBatch.DrawString(Font, str, Vector2.Zero, Color.White);

            spriteBatch.End();
            base.Draw(gameTime);
        }




    }
}